package com.bangdao.hu.roomba.RoutePlan;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;


public class RoutePlanner {
	/*private int[][] grid=  {{0, 0, 1, 0, 0, 0},
							{0, 0, 1, 0, 0, 0},
							{0, 0, 1, 0, 1, 0},
							{0, 0, 1, 0, 1, 0},
							{0, 0, 0, 0, 1, 0}}; */
	private int[][]grid =  {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
							{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
							};
	
	private int[] init = {0, 0};
	
	//private int[] goal = {grid.length -1, grid[0].length-1};
	
	private int[] goal ={10,40};
	
	private int[][] direction = {{-1, 0},  //go up
							     {0, -1},  //go left
							     {1, 0},   //go down
							     {0, 1}};  //go right
	
	private String[] direction_name = {"^", "<", "v", ">"};
	
	private int cost = 1;
	
	private static RoutePlanner instance = null;
	
	Integer[][] expand2 = new Integer[grid.length][grid[0].length];
	String[][] policy2 = new String[grid.length][grid[0].length];
	
    protected RoutePlanner() {
  
//    	robot = new Robot();
     }
    
    public void setInit(int x,int y){
    	init[0]=x;
    	init[1]=y;
    	System.out.println("INIT"+x+y);
    }
    
    public void setDest(int x,int y){
    	goal[0]=x;
    	goal[1]=y;
    	System.out.println("DEST"+x+y);
    }
    
    public static RoutePlanner getInstance() {
      if(instance == null) {
         instance = new RoutePlanner();
      }
      return instance;
    }
    
    public void testSortArray()
    {
    	List<int[]> open = new ArrayList<int[]>();
    	open.add(new int[]{6,3,4});
    	open.add(new int[]{8,3,4});
    	open.add(new int[]{2,3,4});
    	open.add(new int[]{6,3,4});
    	open.add(new int[]{2,3,4});
    	Collections.sort(open, new Comparator<int[]>(){
    		public int compare(int[] a1, int[] a2) {
       			return a1[0] - a2[0];
    		}	
    	});

//    	for(int i = 0; i< 5; ++i)
//    	{
//    		System.out.println(open.get(i)[0] + " " + open.get(i)[1] + " " + open.get(i)[2]);
//    	}
    }
    
    public class IntArraycoomparator implements Comparator<int[]>
    {

		public int compare(int[] a1, int[] a2) {
			
			return a1[0] - a2[0];
		}	
    }
    
    private <T> void initArray(T[][] array, T initVal)
    {
    	for(int row = 0; row < array.length; ++row)
		{
			for(int col = 0; col < array[0].length; ++col)
			{
				array[row][col] = initVal;
			}
		}
    }
    
    private void setupHeuristicMap(Integer[][] h)
    {
    	initArray(h, 0); // TODO: implement this method
    }
	
	public int[] search()
	{
		boolean found = false;
		boolean resign = false;
		
		Integer[][] closed = new Integer[grid.length][grid[0].length];
		initArray(closed, 0);
		Integer[][] action = new Integer[grid.length][grid[0].length];
		initArray(action, 0);
		
		Integer[][] heuristic = new Integer[grid.length][grid[0].length];
		setupHeuristicMap(heuristic);
		Integer[][] expand = new Integer[grid.length][grid[0].length];
		initArray(expand, -1);
//		for(int row = 0; row < grid.length; ++row)
//		{
//			for(int col = 0; col < grid[0].length; ++col)
//			{
//				closed[row][col] = 0;
//			}
//		}
		closed[init[0]][init[1]] = 1;
		
		int	x = init[0];
		int y = init[1];
		int g = 0;
		int f = 0;
		int h = heuristic[x][y];
		int count = 0;
		
		List<int[]> open = new ArrayList<int[]>();
		int[] element = {f, g, h, x, y};
		open.add(element);
		int[] next = null;
//		Collections.sort(open);	
		
		while(!(found || resign))
		{
			if(open.size() == 0)
			{
				resign = true;
				System.out.println("FAIL"+"fail to find goal!");
				return null;
			}else{
//				open.sort(); //Done: find the right way to sort the interger array list
				Collections.sort(open, new Comparator<int[]>(){
					public int compare(int[] lhs, int[] rhs) {
						return lhs[0] - rhs[1];
					}
				});
			    next = (int[]) open.get(0); 
			    open.remove(0);	
				x = next[3];
				y = next[4];
				g = next[1];
				expand[x][y] = count;
				count++;
			
				if(x == goal[0] && y == goal[1])
				{	
					found = true;					
				}else{
					for(int i = 0; i< direction.length; ++i)
					{
						int x2 = x + direction[i][0];
						int y2 = y + direction[i][1];
						if(x2 >= 0 && x2 < grid.length && y2 >= 0 && y2 < grid[0].length)
						{
							if(closed[x2][y2] == 0 && grid[x2][y2] == 0)	//not expanded and not the border
							{
								int g2 = g + cost;
								int h2 = heuristic[x2][y2];
								int f2 = g2 + h2;
								int[] element2 = {f2, g2, h2, x2, y2};
								open.add(element2);		//add the new cell in to open list
								closed[x2][y2] = 1;		//close the cell
								action[x2][y2] = i;
							
							}
						}
					}
				}
			}
		}
		String[][] policy = new String[grid.length][grid[0].length];
		initArray(policy, " ");

		x = goal[0];
		y = goal[1];
		policy[x][y] = "*";
		int x2;
		int y2;
		while(x != init[0] || y != init[1] )
		{
			x2 = x - direction[action[x][y]][0];
		    y2 = y - direction[action[x][y]][1];
		    policy[x2][y2] = direction_name[action[x][y]];
		    x = x2;
		    y = y2;
		}
		
		printArray(policy);
		//printArrayMinus(expand);
		policy2 = policy.clone();
		expand2=expand.clone();
		return next;	
	}
	
	public void testSmooth()
	{
		Integer[][] path = {{0, 0},
					   {0, 1},
					   {0, 2},
					   {1, 2},
					   {2, 2},
					   {3, 2},
					   {4, 2},
					   {4, 3},
					   {4, 4}};
		Integer[][] newpath = smooth(path, 0.1, 0);
		printArray(newpath);
	}
	
	private Integer[][] smooth(Integer[][] path, double weight_data, double weight_smooth)
	{
		Integer[][] newPath = path;
		double tolerance = 1;
		double change = tolerance;
		while(change >= tolerance)
		{
			change = 0.0;
			for(int i = 1; i<path.length-1; ++i)
			{
				for(int j = 0; j < path[0].length; ++j)
				{
					int aux = newPath[i][j];
					newPath[i][j] = (int) (newPath[i][j] + weight_data*(path[i][j] - newPath[i][j]));
					newPath[i][j] = (int) (newPath[i][j] + weight_smooth*(newPath[i-1][j] + newPath[i+1][j] - 2*newPath[i][j]));
					change += Math.abs(aux - newPath[i][j]);
				}
			}
		}
		return newPath;
		
	}
	
	private <T> void printArray(T[][] array)
	{
		for(int row = 0; row < array.length; ++row)
		{
			for(int col = 0; col < array[0].length; ++col)
			{
				System.out.print(array[row][col] + " ");
			}
			System.out.println();
		}
	}
	
	private <T> void printArrayMinus(T[][] array)
	{
		for(int row = 0; row < array.length; ++row)
		{
			for(int col = 0; col < array[0].length; ++col)
			{
				System.out.print(array[row][col] + " ");
			}
			System.out.println("");
		}
	}
	
	public String driveInstruction()
	{
		String directive="";
		if (expand2.length!=policy2.length){
			System.out.println("Expand not equals Policy!!!");
		}else{
			int steps= expand2[goal[0]][goal[1]];
			
			int y=init[0];
			int x=init[1];
			char direct=policy2[y][x].charAt(0);
			System.out.println(direct+"??");
			for (int i=0;i<steps;++i){
				
				switch (direct)
				{
				case '^':

					y=y-1;
					direct=policy2[y][x].charAt(0); 
					if (direct=='v')
					{
						directive=directive+"forward,left,left,";
					}else if(direct=='<'){
						directive=directive+"forward,left,";
					}else if(direct=='>'){
						directive=directive+"forward,right,";
					}else if(direct=='^')
					{
						directive=directive+"forward,";
					}
					
					break;
					
				case 'v':

					y=y+1;
					direct=policy2[y][x].charAt(0); 
					if (direct=='^'){
						directive=directive+"forward,right,right,";
					}else if(direct=='>'){
						directive=directive+"forward,left,";
					}else if(direct=='<'){
						directive=directive+"forward,right,";
					}else if(direct=='v')
					{
						directive=directive+"forward,";
					}
					break;
				case '<':

					x=x-1;
					direct=policy2[y][x].charAt(0); 
					if (direct == '<'){
						directive=directive+"forward,";
					}else if(direct=='>'){
						directive=directive+"forward,right,right,";
					}else if(direct=='^'){
						directive=directive+"forward,right,";
					}else if(direct=='v'){
						directive=directive+"forward,left,";
					}
					break;
				case '>':
					
					x=x+1;
					direct=policy2[y][x].charAt(0);

					if (direct=='>'){
						directive=directive+"forward,";
					}else if(direct=='<'){
						directive=directive+"forward,left,left,";
					}else if(direct=='^'){
						directive=directive+"forward,left,";
					}else if(direct=='v'){
						directive=directive+"forward,right,";
					}else if(direct==' '){
						directive=directive+"forward,";
					}else{
						directive=directive+"forward,";
					}
					break;
				default:
					System.out.println("No Drive Instruction");
					break;
				}
			}
		}
		return directive;
	}
}


